#include "video.h"

#include <stdlib.h>
#include <tox/toxav.h>
#include <vpx/vpx_codec.h>
#include <vpx/vpx_image.h>


void yuv420tobgr(uint16_t width, uint16_t height, const uint8_t *y, const uint8_t *u, const uint8_t *v,
                 unsigned int ystride, unsigned int ustride, unsigned int vstride, uint8_t *out) {
    for (unsigned long int i = 0; i < height; ++i) {
        for (unsigned long int j = 0; j < width; ++j) {
            uint8_t *point = out + 4 * ((i * width) + j);
            int       t_y   = y[((i * ystride) + j)];
            const int t_u   = u[(((i / 2) * ustride) + (j / 2))];
            const int t_v   = v[(((i / 2) * vstride) + (j / 2))];
            t_y            = t_y < 16 ? 16 : t_y;

            const int r = (298 * (t_y - 16) + 409 * (t_v - 128) + 128) >> 8;
            const int g = (298 * (t_y - 16) - 100 * (t_u - 128) - 208 * (t_v - 128) + 128) >> 8;
            const int b = (298 * (t_y - 16) + 516 * (t_u - 128) + 128) >> 8;

            point[2] = r > 255 ? 255 : r < 0 ? 0 : r;
            point[1] = g > 255 ? 255 : g < 0 ? 0 : g;
            point[0] = b > 255 ? 255 : b < 0 ? 0 : b;
            point[3] = ~0;
        }
    }
}

void yuv422to420(uint8_t *plane_y, uint8_t *plane_u, uint8_t *plane_v, uint8_t *input, uint16_t width, uint16_t height) {
    const uint8_t *end = input + width * height * 2;
    while (input != end) {
        uint8_t *line_end = input + width * 2;
        while (input != line_end) {
            *plane_y++ = *input++;
            *plane_v++ = *input++;
            *plane_y++ = *input++;
            *plane_u++ = *input++;
        }

        line_end = input + width * 2;
        while (input != line_end) {
            *plane_y++ = *input++;
            input++; // u
            *plane_y++ = *input++;
            input++; // v
        }
    }
}

static uint8_t rgb_to_y(int r, int g, int b) {
    const int y = ((9798 * r + 19235 * g + 3736 * b) >> 15);
    return y > 255 ? 255 : y < 0 ? 0 : y;
}

static uint8_t rgb_to_u(int r, int g, int b) {
    const int u = ((-5538 * r + -10846 * g + 16351 * b) >> 15) + 128;
    return u > 255 ? 255 : u < 0 ? 0 : u;
}

static uint8_t rgb_to_v(int r, int g, int b) {
    const int v = ((16351 * r + -13697 * g + -2664 * b) >> 15) + 128;
    return v > 255 ? 255 : v < 0 ? 0 : v;
}

void bgrtoyuv420(uint8_t *plane_y, uint8_t *plane_u, uint8_t *plane_v, uint8_t *rgb, uint16_t width, uint16_t height) {
    uint8_t *p;
    uint8_t  r, g, b;

    for (uint16_t y = 0; y != height; y += 2) {
        p = rgb;
        for (uint16_t x = 0; x != width; x++) {
            b          = *rgb++;
            g          = *rgb++;
            r          = *rgb++;
            *plane_y++ = rgb_to_y(r, g, b);
        }

        for (uint16_t x = 0; x != width / 2; x++) {
            b          = *rgb++;
            g          = *rgb++;
            r          = *rgb++;
            *plane_y++ = rgb_to_y(r, g, b);

            b          = *rgb++;
            g          = *rgb++;
            r          = *rgb++;
            *plane_y++ = rgb_to_y(r, g, b);

            b = ((int)b + (int)*(rgb - 6) + (int)*p + (int)*(p + 3) + 2) / 4;
            p++;
            g = ((int)g + (int)*(rgb - 5) + (int)*p + (int)*(p + 3) + 2) / 4;
            p++;
            r = ((int)r + (int)*(rgb - 4) + (int)*p + (int)*(p + 3) + 2) / 4;
            p++;

            *plane_u++ = rgb_to_u(r, g, b);
            *plane_v++ = rgb_to_v(r, g, b);

            p += 3;
        }
    }
}

void bgrxtoyuv420(uint8_t *plane_y, uint8_t *plane_u, uint8_t *plane_v, uint8_t *rgb, uint16_t width, uint16_t height) {
    uint8_t *p;
    uint8_t  r, g, b;

    for (uint16_t y = 0; y != height; y += 2) {
        p = rgb;
        for (uint16_t x = 0; x != width; x++) {
            b = *rgb++;
            g = *rgb++;
            r = *rgb++;
            rgb++;

            *plane_y++ = rgb_to_y(r, g, b);
        }

        for (uint16_t x = 0; x != width / 2; x++) {
            b = *rgb++;
            g = *rgb++;
            r = *rgb++;
            rgb++;

            *plane_y++ = rgb_to_y(r, g, b);

            b = *rgb++;
            g = *rgb++;
            r = *rgb++;
            rgb++;

            *plane_y++ = rgb_to_y(r, g, b);

            b = ((int)b + (int)*(rgb - 8) + (int)*p + (int)*(p + 4) + 2) / 4;
            p++;
            g = ((int)g + (int)*(rgb - 7) + (int)*p + (int)*(p + 4) + 2) / 4;
            p++;
            r = ((int)r + (int)*(rgb - 6) + (int)*p + (int)*(p + 4) + 2) / 4;
            p++;
            p++;

            *plane_u++ = rgb_to_u(r, g, b);
            *plane_v++ = rgb_to_v(r, g, b);

            p += 4;
        }
    }
}

void scale_rgbx_image(uint8_t *old_rgbx, uint16_t old_width, uint16_t old_height, uint8_t *new_rgbx, uint16_t new_width,
                      uint16_t new_height) {
    for (int y = 0; y != new_height; y++) {
        const int y0 = y * old_height / new_height;
        for (int x = 0; x != new_width; x++) {
            const int x0 = x * old_width / new_width;

            const int a         = x + y * new_width;
            const int b         = x0 + y0 * old_width;
            new_rgbx[a * 4]     = old_rgbx[b * 4];
            new_rgbx[a * 4 + 1] = old_rgbx[b * 4 + 1];
            new_rgbx[a * 4 + 2] = old_rgbx[b * 4 + 2];
        }
    }
}
